#include <sdkeli_ls/sdkeli_ls_common.h>

#include <cstdio>
#include <cstring>

namespace sdkeli_ls
{
	CSDKeliLsCommon::CSDKeliLsCommon(CParserBase *parser) : 
		mDiagPublisher(NULL),
		dExpectedFreq(15.0), /* Default frequency */
		mParser(parser)
	{
		/*Initialize receive buffer*/
		memset(mRecvBuffer, RECV_BUFFER_SIZE, 0);
		mDataLength = 0;

		/*Set reconfigure callback*/
		dynamic_reconfigure::Server<sdkeli_ls::SDKeliLsConfig>::CallbackType f;
		f = boost::bind(&sdkeli_ls::CSDKeliLsCommon::UpdateConfig, this, _1, _2);
		mDynaReconfigServer.setCallback(f);

		/*Set data publisher (used for debug)*/
		ros::NodeHandle ndHomePublisher("~"); // for config parameter...
		ndHomePublisher.param<bool>("publish_datagram", mPublishData, false);
		if(mPublishData)
		{
			/*datagram publish is enabled*/
			mDataPublisher = mNodeHandler.advertise<std_msgs::String>("datagram", 1000);
		}

		/*Set scan publisher*/
		mScanPublisher = mNodeHandler.advertise<sensor_msgs::LaserScan>("scan", 1000);

		mDiagUpdater.setHardwareID("none");
		mDiagPublisher = new diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>(mScanPublisher, 
				mDiagUpdater,
				/* frequency should be target +- 10% */
				diagnostic_updater::FrequencyStatusParam(&dExpectedFreq, &dExpectedFreq, 0.1, 10),
				/*timestamp delta can be from 1.1 to 1.3x what it ideally is*/
				diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0 / dExpectedFreq - mConfig.time_offset));

		ROS_ASSERT(mDiagPublisher);
	}

	int CSDKeliLsCommon::StopScanner()
	{
		int result = 0;

#ifdef CMD_STOP_STREAM_DATA /* TODO: Enable following code block when stop command defined. */
		result = SendDeviceReq(CMD_STOP_STREAM_DATA, NULL);
		if(0 != result)
		{
			// use printf because we couldn't use ROS_ERROR from destructor
			printf("STOP Scan ERROR!\n");
		}
		else
		{
			printf("Streaming scan data stopped.\n");
		}
#endif
		return result;
	}

	bool CSDKeliLsCommon::RebootDevice()
	{
#ifdef CMD_REBOOT_DEVICE /*TODO: Enable following code block when commands defined.*/
		/*Set maintenance access mode to allow reboot to be sent*/
		std::vector<unsigned char> respAccess;
		int result = SendDeviceReq(CMD_SET_MAINTENANCE_ACCESS_MODE, &respAccess);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Error setting access mode");
			mDiagUpdater.broadcast(diagnostic_msgs::DisgnosticStatus::ERROR,
					"SDKELI_LS - Error setting access mode");

			return false;
		}

		std::string strAccessResp = StringResp(respAccess);
		if(strAccessResp != "sAN SetAccessMode 1")
		{
			ROS_ERROR_STREAM("SDKELI_LS - Error setting access mode, unexpected response : " << strAccessResp);
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI - Error setting access mode.");

			return false;
		}

		/*send reboot command*/
		std::vector<unsigned char> respReboot
		result = SendDeviceReq(CMD_REBOOT, &respReboot);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Error rebooting device");
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Error rebooting device");

			return false;
		}

		std::string strRebootResp = StringResp(respReboot);
		if(strRebootResp != "sAN mSCreboot")
		{
			ROS_ERROR_STREAM("SDKELI_LS - Error setting access mode, unexpected response : " << strRebootResp);
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Error rebooting device");

			return false;
		}

		ROS_INFO("SDKELI_LS - Rebooted scanner");
#endif
		return true;
	}

	CSDKeliLsCommon::~CSDKeliLsCommon()
	{
		delete mDiagPublisher;
		printf("sdkeli_ls drvier exiting.\n");
	}

	int CSDKeliLsCommon::Init()
	{
		int result = InitDevice();
		if(0 != result)
		{
			ROS_FATAL("Failed to init device: %d", result);
			return result;
		}

		result = InitScanner();
		if(0 != result)
		{
			ROS_FATAL("Failed to init scanner: %d", result);
		}

		return result;
	}
	
	int CSDKeliLsCommon::InitScanner()
	{
#ifdef CMD_DEVICE_INFO /*TODO: Enable following code block when command defined*/
		/*Read device identify*/
		std::vector<unsigned char> respIdentify;
		int result = SendDeviceReq(CMD_READ_IDENTIFY, &respIdentify);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Error reading variable 'DeviceIdent'.");
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Error reading variable 'DeviceIdent'.");
		}

		/*Read device variable 'SerialNumber' by name.*/
		std::vector<unsigned char> respSerialNumber;
		result = SendDeviceReq(CMD_READ_SERIAL_NUMBER, &respSerialNumber);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Error reading variable 'SerialNumber'.");
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Error reading variable 'SerialNumber'.");
		}

		/*Set hardware ID based on device identify and serial number*/
		std::string strIdentify     = StringResponse(respIdentify);
		std::string strSerialNumber = StringResponse(respSerialNumber);
		mDiagUpdater.setHardwareID(strIdentify + " " + strSerialNumber);
		
		if(!IsCompatibleDevice(strIdentify))
		{
			ROS_ERROR("SDKELI_LS - Error Unsuppored identify %s", strIdentify);
			return ExitFatal;
		}

		/*Read device variable 'FirmwareVersion' by name.*/
		result = SendDeviceReq(CMD_READ_FIRMWARE_VERSION, NULL);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Error reading variable 'FirmwareVersion'.");
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Error reading variable 'FirmwareVersion'.");
		}

		/*Read Device State*/
		std::vector<unsigned char> respDeviceState;
		result = SendDeviceReq(CMD_READ_DEVICE_STATE, &respDeviceState);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Error reading variable 'devicestate'.");
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Error reading variable 'devicestate'.");
		}
		std::string strDeviceState = StringResponse(respDeviceState);

		/*Check device state:
		 * 0: Busy,
		 * 1: Ready,
		 * 2: Error */
		if(strDeviceState == "sRA SCdevicestate 0")
		{
			ROS_WARN("Laser scanner is busy.");
		}
		else if(strDeviceState == "sRA SCdevicestate 1")
		{
			ROS_DEBUG("Laser scanner is ready.");
		}
		else if(strDeviceState == "sRA SCdevicedstate 2")
		{
			ROS_ERROR_STREAM("Laser scanner error state: " << strDeviceState);
			if(mConfig.auto_reboot)
			{
				rebootDevice();
			}
		}
		else
		{
			ROS_WARN_STREAM("Laser scanner reports unknown devicestate: " << strDeviceState);
		}

		/*Start data streaming*/
		result = SendDeviceReq(CMD_START_STREAM_DATA, NULL);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Error when starting streaming 'LMDscandata'.");
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Error when starting streaming 'LMDscandata'.");

			return ExitError;
		}
#endif
		return ExitSuccess;
	}

	std::string sdkeli_ls::CSDKeliLsCommon::StringResp(const std::vector<unsigned char> &resp)
	{
		std::string strResp;
		for(std::vector<unsigned char>::const_iterator it = resp.begin();
				it != resp.end();
				it++)
		{
			if(*it > 13)
			{
				strResp.push_back(*it);
			}
		}

		return strResp;
	}

	bool sdkeli_ls::CSDKeliLsCommon::IsCompatibleDevice(const std::string strIdentify) const
	{
		// TODO: Always return true
		return true;
	}

	int CSDKeliLsCommon::LoopOnce()
	{
		mDiagUpdater.update();

		int dataLength = 0;
		static unsigned int iteration_count = 0;

		int result = GetDataGram(mRecvBuffer + mDataLength, RECV_BUFFER_SIZE - mDataLength, &dataLength);
		if(0 != result)
		{
			ROS_ERROR("SDKELI_LS - Read Error when getting datagram: %d", result);
			mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR,
					"SDKELI_LS - Read Error when getting datagram.");

			return ExitError;
		}
		else
		{
			ROS_DEBUG("SDKELI_LS - Received data gram. Data Length %d", dataLength);
			mDataLength += dataLength;
		}

		if(mDataLength < FRAME_LENGTH) /*Fixed data length of 1630*/
		{
			// ROS_WARN("SDKELI_LS - Failed to read data gram");
			return ExitSuccess; /*return success to continue looping*/
		}
		else
		{
			ROS_DEBUG("SDKELI_LS - Received one frame. Total Length %d", mDataLength);
			if(mDataLength % FRAME_LENGTH != 0)
			{
				ROS_ERROR("SDKELI_LS - Invalid data length!");
			}
		}

		/*Data requested, skip frames*/
		if(iteration_count++ % (mConfig.skip + 1) != 0)
		{
			ROS_INFO("SDKELI_LS - Skip frame");
			return ExitSuccess;
		}

		/*One full frame received. Start Data processing...*/
		if(mPublishData)
		{
			std_msgs::String data_msg;
			data_msg.data = std::string(reinterpret_cast<char *>(mRecvBuffer));
			mDataPublisher.publish(data_msg);
		}

		sensor_msgs::LaserScan msg;

		/*data are started with 0xaa, and data length is fixed size: 1630*/
		char *posBuffer = (char *)mRecvBuffer;
		int  startIndex = 0;
		char *start = strchr(posBuffer + startIndex, STX);
		if(start == NULL)
		{
			ROS_ERROR("SDKELI_LS - Invalide data! Header NOT found!");
		}

		char *end   = NULL;
		while(start != NULL && (startIndex + FRAME_LENGTH <= mDataLength))
		{
			size_t length = FRAME_LENGTH;
			end = start + length; /*Fixed total length 1631*/
			*end = '\0';

			int success = mParser->Parse(start, length, mConfig, msg);
			if(ExitSuccess == success)
			{
				if(mConfig.debug_mode)
				{
					DumpLaserMessage(msg);
				}
				mDiagPublisher->publish(msg);
			}

			posBuffer = end + 1;
			start     = strchr(posBuffer, STX);
			startIndex += FRAME_LENGTH;
		}

		memset(mRecvBuffer, RECV_BUFFER_SIZE, 0);
		mDataLength = 0;

		return ExitSuccess; // return success to continue
	}

	void CSDKeliLsCommon::CheckAngleRange(sdkeli_ls::SDKeliLsConfig &config)
	{
		if(config.min_ang > config.max_ang)
		{
			ROS_WARN("Minimum angle must be greater than maxmum angle. Adjusting min_ang");
			config.min_ang = config.max_ang;
		}
	}

	void CSDKeliLsCommon::UpdateConfig(sdkeli_ls::SDKeliLsConfig &newConfig, uint32_t level)
	{
		CheckAngleRange(newConfig);
		mConfig = newConfig;
	}

	void CSDKeliLsCommon::DumpLaserMessage(sensor_msgs::LaserScan &msg)
	{
		ROS_DEBUG("Laser Message to send:");
		ROS_DEBUG("Header  frame_id: %s", msg.header.frame_id.c_str());
		//ROS_DEBUG("Header timestamp: %ld", msg.header.stamp);
		ROS_DEBUG("angle_min: %f", msg.angle_min);
		ROS_DEBUG("angle_max: %f", msg.angle_max);
		ROS_DEBUG("angle_increment: %f", msg.angle_increment);
		ROS_DEBUG("time_increment: %f", msg.time_increment);
		ROS_DEBUG("scan_time: %f", msg.scan_time);
		ROS_DEBUG("range_min: %f", msg.range_min);
		ROS_DEBUG("range_max: %f", msg.range_max);
	}
} // sdkeli_ls
